#include <QString>
#include <tctracker.hpp>

#include <stdio.h>
#include <stdlib.h>

#include <limits.h>

#include <qwt_plot_curve.h>
#include <qwt_series_data.h>
#include <BlobResult.h>

#define M_4_PI 4 * 3.41592654

QColor
TCTracker::m_Colors[COLOR_COUNT] = {
    QColor(255, 0, 0),
    QColor(0, 255, 0),
    QColor(0, 0, 255),
    QColor(255, 255, 0),
    QColor(0, 255, 255),
    QColor(255, 0, 255),
    QColor(255, 128, 0),
    QColor(255, 0, 128),
    QColor(128, 255, 0),
    QColor(128, 128, 128)
};

typedef enum xps_savitzky_golay_order_t {
    XPS_SAVITZKY_GOLAY_QUADRATIC,
    XPS_SAVITZKY_GOLAY_QUARTIC,
    XPS_SAVITZKY_GOLAY_INVALID
} xps_savitzky_golay_order_t;


/* Savitzky-Golay -- Quadratic-Cubic */
static const double
SGQC[11][13] = {
    {-253.0, -138.0, -33.0, 62.0, 147.0, 222.0, 287.0, 322.0, 387.0, 422.0, 447.0, 462.0, 467.0},
    { -42.0,  -21.0,  -2.0, 15.0,  30.0,  43.0,  54.0,  63.0,  70.0,  75.0,  78.0,  79.0,   0.0},
    {-171.0,  -76.0,   9.0, 84.0, 149.0, 204.0, 249.0, 284.0, 309.0, 324.0, 329.0,   0.0,   0.0},
    {-136.6,  -51.0,  24.0, 89.0, 144.0, 189.0, 224.0, 249.0, 264.0, 269.0,   0.0,   0.0,   0.0},
    { -21.0,   -6.0,   7.0, 18.0,  27.0,  34.0,  39.0,  42.0,  43.0,   0.0,   0.0,   0.0,   0.0},
    { -78.0,  -13.0,  42.0, 87.0, 122.0, 147.0, 162.0, 167.0,   0.0,   0.0,   0.0,   0.0,   0.0},
    { -11.0,    0.0,   9.0, 16.0,  21.0,  24.0,  25.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0},
    { -36.0,    9.0,  44.0, 69.0,  84.0,  89.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0},
    { -21.0,   14.0,  39.0, 54.0,  59.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0},
    {  -2.0,    3.0,   6.0,  7.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0},
    {  -3.0,   12.0,  17.0,  0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0}
};

static const double
SGQCNORM[11] = { 5175.0, 805.0,   3059.0, 2261.0,  323.0,  1105.0,  143.0, 429.0, 231.0,  21.0, 35.0};

static double *
xps_smooth_savitzky_golay(const double *y, int m, unsigned int size)
{
    unsigned int      i      = 0;
    unsigned int      j      = 0;
    unsigned int      jump   = 0;
    unsigned int      index  = 0;
    double           *ptr    = NULL;
    double           *result = NULL;

    jump   = (m - 1) / 2;
    index  = (25 - m) / 2;
    result = new double[size];

    if (!result)
        return  NULL;

    memcpy(result, y, size * sizeof(double));

    ptr  = result;
    ptr += jump;
    for (i = 0 ; i < size - m - 1 ; i++) {
        for (j = 0 ; j < jump ; j++)
            *ptr += SGQC[index][j] * (y[j] + y[m - 1 - j]);

        *ptr = (*ptr + SGQC[index][j] * y[j]) / SGQCNORM[index];
        if (*ptr < 0)
            *ptr = 0.0;

        ptr++;
        y++;
    }

    return result;
}

TCInsect::TCInsect() :
    m_Position(0.0, 0.0),
    m_Velocity(0.0, 0.0),
    m_Visible(true),
    m_Checked(false)
{
}

QPointF
TCInsect::position() const
{
    return m_Position;
}

QPointF
TCInsect::velocity() const
{
    return m_Velocity;
}

bool
TCInsect::visible() const
{
    return m_Visible;
}

bool
TCInsect::checked() const
{
    return m_Checked;
}

void
TCInsect::setPosition(const QPointF &position)
{
    m_Position = position;
}

void
TCInsect::setVelocity(const QPointF &velocity)
{
    m_Velocity = velocity;
}

void
TCInsect::setChecked(bool checked)
{
    m_Checked = checked;
}

void
TCInsect::setVisible(bool visible)
{
    m_Visible = visible;
}

void
TCInsect::setArea(float area)
{
    m_Area = area;
}

float
TCInsect::area() const
{
    return m_Area;
}

TCTracker::TCTracker(int fps, QObject *parent) :
    QObject(parent),
    m_Plot(NULL),
    m_Insects(new QMap<int,TCInsect*>),
    m_ColorId(0),
    m_MPS(30),
    m_FPS(fps),
    m_cMPS(0)
{
}

TCTracker::~TCTracker()
{
    m_Samples.clear();
    m_Data.clear();
    m_Curve.clear();

    if (m_Insects)
        delete m_Insects;
    m_Insects = NULL;
}

void
TCTracker::setPlotter(QwtPlot *plot)
{
    m_Plot = plot;
}

void
TCTracker::setColors(const QColor &)
{
}

#define SQR(x) ((x) * (x))
#define DISTANCE(p1, p2) \
    sqrt(SQR((p1).x() - (p2).x()) + SQR((p1).y() - (p2).y()))
#define MODULUS(p) \
    sqrt(SQR((p).x()) + SQR((p).y()))

void
TCTracker::track(unsigned char *data, int width, int height, int bpl)
{
    if (!m_Mutex.tryLock())
        return;

    if (!data || width == 0 || height == 0 || bpl == 0)
        return;

    CBlobResult  blobs;
    IplImage    *image = NULL;
    IplImage    *gray  = NULL;
    CvSize       size  = {width, height};
    QPointF      velocity;
    CvRect       roi;
    CvPoint      topLeft;
    CvPoint      bottomRight;

    image = cvCreateImageHeader(size, IPL_DEPTH_8U, 3);
    gray  = cvCreateImage(size, IPL_DEPTH_8U, 1);

    cvSetData(image, data, bpl);

    cvCvtColor(image, gray, CV_RGB2GRAY);
    cvEqualizeHist(gray, gray);
    cvSmooth(gray, gray, CV_GAUSSIAN, 1, 1, 2, 3);

    roi.x      = image->width * 0.1;
    roi.y      = image->height * 0.1;
    roi.width  = image->width - 2 * roi.x;
    roi.height = image->height - 2 * roi.y;

    cvSetImageROI(gray, roi);
    cvInRangeS(gray, cvScalar(0, 0, 0), cvScalar(1, 0, 0), gray);
    cvRectangle(image,
                cvPoint(roi.x, roi.y),
                cvPoint(roi.width + roi.x, roi.height + roi.y),
                cvScalar(255, 0, 0));

    blobs = CBlobResult(gray, NULL, 0);

    blobs.Filter(blobs, B_EXCLUDE, CBlobGetArea(), B_LESS, 60);
    blobs.Filter(blobs, B_EXCLUDE, CBlobGetArea(), B_GREATER, 200);

    int count = blobs.GetNumBlobs();

    for (int i = 0 ; i < count ; i++) {
        TCInsect *obj;
        CBlob     blob(blobs.GetBlob(i));
        CvRect    bounds(blob.GetBoundingBox());
        QPointF   position;
        int       last = 0;
        float     x;
        float     y;

        topLeft.x     = bounds.x;
        topLeft.y     = bounds.y;
        bottomRight.x = bounds.x + bounds.width;
        bottomRight.y = bounds.y + bounds.height;

        x = ((float)(bottomRight.x + topLeft.x)) / 2.0;
        y = ((float)(bottomRight.y + topLeft.y)) / 2.0;

        position = QPointF(x, -y);
        last     = checkBlobPosition(gray, position);

        if (last == -1) {
            last = m_Insects->count();
            obj  = new TCInsect;

            if (last > 2 * COLOR_COUNT) {
                last = 0;
                m_Insects->clear();
            }

            obj->setArea(blob.Area());
            obj->setPosition(position);
            obj->setVelocity(QPointF(0.0, 0.0));
            obj->setChecked(true);

            m_Insects->insert(last, obj);
        } else {
            QPointF     velocity;

            obj = m_Insects->value(last);

            position = (obj->position() + position) / 2.0;

            velocity = obj->velocity();

            if (obj->visible())
                velocity = (velocity + (position - obj->position())) / 2.0;

            obj->setArea((blob.Area() + obj->area()) / 2.0);
            obj->setPosition(position);
            obj->setVelocity(velocity);
            obj->setChecked(true);

            m_Insects->insert(last, obj);
        }

        if (!obj)
            continue;

        if (obj->visible()) {
            if (m_Plot) {
                if (m_ColorId > COLOR_COUNT)
                    m_ColorId = 0;

                if (!m_Samples.contains(last)) {
                    m_Samples.insert(last, new QVector<QPointF>);
                    m_Data.insert(last, new QwtPointSeriesData);
                    m_Curve.insert(last, new QwtPlotCurve);

                    m_Curve.value(last)->setPen(m_Colors[m_ColorId]);
                    m_ColorId++;
                }

                m_Samples.value(last)->push_back(position);

                m_Data.value(last)->setSamples(*(m_Samples.value(last)));

                m_Curve.value(last)->setData(m_Data.value(last));
                m_Curve.value(last)->attach(m_Plot);
            }
#ifdef Q_OS_LINUX
            char    str[128] = {0};
#endif
            CvFont  font;

            cvInitFont(&font, 1, 0.8, 0.8, 0, 1, 8);
#ifdef Q_OS_LINUX
            snprintf(str, 128, "%d - vx:%.2f vy:%.2f",
                     last,
                     obj->velocity().x(),
                     obj->velocity().y());

            cvPutText(image, str, bottomRight, &font, cvScalar(0, 0, 0));
#endif
            cvRectangle(image, topLeft, bottomRight, cvScalar(0, 0, 255));
            blob.FillBlob(image, cvScalar(0, 255, 0));
        }
    }
    m_cMPS++;

    if (m_cMPS > m_MPS)
        m_cMPS = 0;

    for (int i = 0 ; i < m_Insects->count() ; i++) {
        TCInsect *insect;

        insect = m_Insects->values()[i];

        if (!insect)
            continue;

        if (!insect->checked())
            insect->setVisible(true);

        insect->setChecked(false);
        m_Insects->insert(m_Insects->keys()[i], m_Insects->values()[i]);
    }

    if (m_Plot)
        m_Plot->replot();

    cvReleaseImageHeader(&image);
    cvReleaseImage(&gray);

    m_Mutex.unlock();
}

int
TCTracker::checkBlobPosition(IplImage *image, const QPointF &position)
{
    int   current = -1;
    float auxDist = 0.0;
    float minDist = (float)LONG_MAX;

    for (int i = 0 ; i < m_Insects->count() ; i++) {
        TCInsect   *insect = m_Insects->values()[i];

        auxDist = DISTANCE(insect->position(), position);
        if (auxDist < minDist) {
            minDist = auxDist;
            current = m_Insects->keys()[i];
        }
    }
    if (minDist > image->width / 20.0)
        return -1;

    return current;
}


bool
TCTracker::saveTrayectories(const QString &basename, const QString &basedir, int m)
{
    if (m % 2 == 0 && m != 0)
        return false;

    double *x  = NULL;
    double *y  = NULL;
    FILE   *fp = NULL;

    for (int i = 0 ; i < m_Samples.count() ; i++) {
        QString      filename;
        int          key;

        key      = m_Samples.keys()[i];
        filename = basedir + "/" + basename + QString("_insecto_%1.cvs").arg(key);

        QByteArray          ba(filename.toLatin1());
        QVector<QPointF>   *data;

        data = m_Samples.value(key);
        fp   = fopen(ba.constData(), "w");

        x = new double[data->size()];
        y = new double[data->size()];

        for (int j = 0 ; j < data->size() ; j++) {
            const QPointF &point(data->at(j));

            x[j] = point.x();
            y[j] = point.y();
        }

        if (m == 0) {
            for (int j = 0 ; j < data->size() ; j++)
                fprintf(fp, "%.3f, %.3f\n", x[j], y[j]);
        } else {
            double *smooth = NULL;

            smooth = xps_smooth_savitzky_golay(y, m, data->size());

            if (!smooth)
                goto failed;

            for (int j = 0 ; j < data->size() ; j++)
                fprintf(fp, "%.3f, %.3f\n", x[j], smooth[j]);

            delete[] smooth;
        }

        delete[] x;
        delete[] y;

        x  = NULL;
        y  = NULL;

        fclose(fp);
        fp = NULL;
    }

    return true;

failed:
    if (fp)
        fclose(fp);

    if (x)
        delete[] x;

    if (y)
        delete[] y;
    return false;
}

void
TCTracker::setMPS(int mps)
{
    m_MPS = mps;
}

void
TCTracker::setFPS(int fps)
{
    m_FPS = fps;
}

void
TCTracker::clearPlot()
{
    m_Samples.clear();
    m_Insects->clear();
    m_Curve.clear();

    m_Plot->detachItems();
}
